//-----------------------------------------------------------------------
//
//  Hemisson Comms Control
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//-----------------------------------------------------------------------

//using Microsoft.Ccr.Core;
//using Microsoft.Dss.Core;

using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;
using ProMRDS.Robotics.Hemisson;
using System.Collections;


using brick = ProMRDS.Robotics.GenericBrick.Proxy;
using Microsoft.Ccr.Core;


// Hemisson Commands (extracted from the OS source code)
// The format is command,parameter,parameter,...CRLF
// The intervening commas are ignored by most functions, but they must be
// there as place holders.
// The response is to echo the command in lower case followed by the data
// (if applicable) and ending with CRLF.
//
// A	unused
// B	HemiOS_v_%d.%d
// C	unused
// D	Set Motor Speeds
// E	Read Motor Speeds
// F	unused
// G	unused
// H	Beep the Buzzer with 0=Off or 1=On
// I	Read Switches - Returns four 0 or 1 values
// J	Scan I2C bus
// K	unused
// L	Set LEDs with four 0 or 1 parameters: On/Off, Pgm/Exec, FrontLeft, FrontRight
//      The first LED cannot be changed and always blinks. The second is red and is
//      at the back right. 
// M	Get Brightness Zone Sensors (0=Front, 1=Rear, 2=Ground)
// N	Get All Proximity Sensors (IR)
// O	Get All Brightness
// P	Get Proximity Zone Sensors
// Q	Webots
// R	Read I2C
// S	Webots
// T	Read TV Remote
// U	Webots
// V	unused
// W	Write I2C
// X	Webots
// Y	Write I2C (pointer method)
// Z	Reset
// &	Fast Binary Read
// *	Fast Binary Write (motors, output pins)
// !	Check if sensors refreshed
// 1	Read RB0,RB6,RB7
// 2	Write RB0,RB6,RB7


namespace ProMRDS.Robotics.HemissonComms
{
    /// <summary>
    /// Hemisson Control (Communications)
    /// All concurrency is done by calling the service, not this module.
    /// Assume this is not thread safe.
    /// </summary>
    internal class HemissonControl
    {

        SerialPort _serialPort;
        brick.GenericBrickOperations _brickPort = null;

        // Port for sending commands to
        public Port<Command> CommandPort = new Port<Command>();

        // Use our own dispatcher because we block on I/O
        Dispatcher _dispatcher;
        DispatcherQueue _taskQueue;

        // Response from the Hemisson to a command packet
        private const int MAX_RESPONSE_LENGTH = 80;
        byte[] _response = new byte[MAX_RESPONSE_LENGTH];
        int _responseCount = 0;

        // IR values have to change more than this before a
        // notification is triggered
        const int IR_NOTIFICATION_THRESHOLD = 5;
        // Values higher than this are going to be seen as a
        // "bump" of the sensor
        const int IR_STATE_THRESHOLD = 32;

        bool Connected = false;

        public int Delay = 100;

        public int MessagesSentCounter = 0;
        public int ErrorCounter = 0;

        public const int IR_COUNT = 8;
        public const int SWITCH_COUNT = 4;

        // Array of IR sensors
        // NOTE: The order of values in this array must match the order of
        // the values returned by the Read IR command
        //private int[] priorIRValues = new int[IR_COUNT];
        //public int[] IRValues = new int[IR_COUNT];

        // Array of Switch values
        //private int priorSwitches = 0;
        //public int Switches = 0;

        // Current settings of the LEDs
        public int LEDs = 0;

        //private Queue cmdQueue = new Queue();
        //private Queue syncQueue;


        #region Constructor and Destructor

        public HemissonControl(brick.GenericBrickOperations brickPort)
        {
            int i;

            _brickPort = brickPort;

            /*
            // Note that the prior values are initialised so that
            // there will be a notification on the very first update
            for (i=0; i<IR_COUNT; i++)
            {
                priorIRValues[i] = -IR_NOTIFICATION_THRESHOLD;
                IRValues[i] = 0;
            }
            */

            // Explicitly create a Dispatcher so we can control the pool size
            _dispatcher = new Dispatcher(3, "Comms Pool");
            _taskQueue = new DispatcherQueue("Comms Queue", _dispatcher);

            // Wait one time for an InternalDrivePower command
            Arbiter.Activate(_taskQueue, Arbiter.ReceiveWithIterator<Command>(false, CommandPort, CommandHandler));
        }

        ~HemissonControl()
        {
            Close();
        }
        #endregion

        #region Serial Port Control

        /// <summary>
        /// Connect to the Robot Brick
        /// </summary>
        /// <param name="cfg"></param>
        /// <param name="errorMessage"></param>
        /// <returns>True for success, False for Failure. Also errorMessage is set if there is a problem</returns>
        public bool Connect(brick.CommsConfig cfg, out string errorMessage)
        {
            if (cfg.SerialPort <= 0)
            {
                errorMessage = "Serial port is not configured";
                return false;
            }

            if (Connected)
                Close();

            try
            {
                string ComPort = "COM" + cfg.SerialPort.ToString();

                // Create a new serial port connection
                // Note that the baud rate and data format are hard-coded
                _serialPort = new SerialPort(ComPort, 115200, Parity.None, 8, StopBits.One);

                // Added for CF version
                // In terms of the baud rate, this is an eternity
                _serialPort.WriteTimeout = cfg.Timeout;
                _serialPort.ReadTimeout = cfg.Timeout;

                Delay = cfg.Delay;

                // Attempt to open the connection
                _serialPort.Open();

                // If we reach here, it was successful
                _serialPort.DiscardInBuffer();
                errorMessage = string.Empty;
                Connected = true;
                return true;
            }
            catch (Exception ex)
            {
                // Error occurred in opening the connection so report it
                errorMessage = string.Format("Error connecting to robot on COM{0}: {1}",
                                    cfg.SerialPort, ex.Message);
                return false;
            }
        }

        /// <summary>
        /// Close Serial Port
        /// </summary>
        public void Close()
        {
            if (!Connected)
                return;
            Connected = false;

            // Wait a little while to make sure we don't kill an
            // operation in progress
            Thread.Sleep(100);
            if (_serialPort != null && _serialPort.IsOpen)
            {
                _serialPort.Close();
                _serialPort = null;
            }

        }

        // IMPORTANT NOTE:
        // This routine runs synchronously and should only be used when the
        // service starts up.
        // The process for connecting is simple:
        // 1. Send a CR to ensure that we are synchronized with the robot
        // 2. Send a Version command
        // 3. Get the response and check that it is valid
        // 4. Parse the version number and return it
        public bool WaitForConnect(brick.Firmware firmware)
        {
            // Send a command to "wake up" the robot
            byte[] buf = new byte[1];
            buf[0] = 13;        // CR
            // NOTE: Sometimes there might be something in the Hemisson's buffer
            // This is why we send a CR first
            // Should get some sort of response, but we don't check
            SendPacket(buf, 0);

            // Send a command to display HemiOS version
            buf[0] = (byte)'B';
            SendPacket(buf, 0);

            // No response?
            if (_responseCount <= 0)
                return false;

            // Extract the version string
            // Ignore the first two characters which are "b,"
            StringBuilder sb = new StringBuilder();
            for (int i = 2; i < _responseCount; i++)
                sb.Append((char)_response[i]);

            string version = sb.ToString();

            // Return the version info
            firmware.VersionString = version;

            // Extract the version number (if possible)
            int offset = version.LastIndexOf(".");
            int begin;
            int minor = 0;
            if (offset >= 0)
            {
                begin = offset - 1;
                for (int i = offset + 1; i < version.Length; i++)
                {
                    if (version[i] < '0' || version[i] > '9')
                        break;
                    minor = minor * 10 + (version[i] - (int)'0');
                }
            }
            else
                begin = version.Length - 1;

            int major = 0;
            for (; begin >= 0; begin--)
            {
                if (version[begin] < '0' || version[begin] > '9')
                    break;
            }
            begin++;
            if ((offset - begin) > 0)
            {
                for (int i = begin; i < offset; i++)
                {
                    if (version[i] < '0' || version[i] > '9')
                        break;
                    major = major * 10 + (version[i] - (int)'0');
                }
            }
            firmware.MajorVersion = major;
            firmware.MinorVersion = minor;

            // Flashes both front LEDs and sounds a tone
            SetLEDs(0x0C);
            SpeakerTone(100, 4000);
            SetLEDs(0);

            return true;
        }

        #endregion

        #region Speed Control

        // Perform the necessary transformation from the MRDS power range
        // of -1.0 to +1.0 to the robot's range which is -9 to +9.
        // NOTE: The values of 0.5 and 0.6 map to the same speed setting.
        int TransformPower(double? power)
        {
            int p;

            if (power == null)
                return 0;

            if (power >= 0)
                p = (int)((power * 9) + 0.5);
            else
                p = (int)((power * 9) - 0.5);

            return p;
        }

        /// <summary>
        /// Set Power to Motors
        /// </summary>
        /// <param name="left">Range is -1.0 to 1.0</param>
        /// <param name="right">Range is -1.0 to 1.0</param>
        public Port<byte[]> SetPower(double? left, double? right)
        {
            int powerLeft, powerRight;
            int temp;

            if ((left != null && (left < -1.0 || left > 1.0)) 
                || ((right != null) && (right < -1.0 || right > 1.0))) 
                throw new SystemException("Invalid Speed!");

            if (left != null)
                temp = TransformPower(left);
            else
                temp = 0;

            powerLeft = temp;

            if (right != null)
                temp = TransformPower(right);
            else
                temp = 0;

            powerRight = temp;

            return SendPower(powerLeft, powerRight);
        }

        // Send the motor speeds to the Hemisson using Fast Binary Write.
        // Speeds range from -9 (backwards) to +9 (forwards) with 0=Stop
        private Port<byte[]> SendPower(int left, int right)
        {
            byte[] buf = new byte[4];
            buf[0] = (byte)'*';
            if (left >= 0)
                buf[1] = (byte)left;
            else
                buf[1] = (byte)(256 + left);
            if (right >= 0)
                buf[2] = (byte)right;
            else
                buf[2] = (byte)(256 + right);

            // Get the LED settings and send them as well
            buf[3] = (byte)LEDs;

            Command cmd = new Command(buf);
            CommandPort.Post(cmd);
            return cmd.ResponsePort;
        }

        /*
        // Sends the motor speeds to the Hemisson using the 'D' command
        // Speeds range from -9 (backwards) to +9 (forwards) with 0=Stop
        private void SendSpeed(int left, int right)
        {
            int n = 5, i;
            if (left < 0)
                n++;
            if (right < 0)
                n++;
            byte[] buf = new byte[n];

            i = 0;
            buf[i++] = (byte)'D';
            buf[i++] = (byte)',';

            if (left < 0)
            {
                buf[i++] = (byte)'-';
                left = -left;
            }
            buf[i++] = (byte)(left + '0');

            buf[i++] = (byte)',';

            if (right < 0)
            {
                buf[i++] = (byte)'-';
                right = -right;
            }
            buf[i++] = (byte)(right + '0');

            Command cmd = new Command(buf);
            CommandPort.Post(cmd);
            return cmd.ResponsePort;
        }
        */

        #endregion

        #region Polling

        /// <summary>
        /// Poll sensors using Fast Binary Read
        /// </summary>
        public Port<byte[]> Poll()
        {
            Command cmd = new Command();

            cmd.CommandString = new byte[1];
            cmd.CommandString[0] = (byte)'&';
            cmd.ResponseLength = 23;
            CommandPort.Post(cmd);
            // Return the response port so the caller can wait
            return cmd.ResponsePort;
        }

        /*
        /// <summary>
        /// Poll the IR Sensors
        /// </summary>
        /// <returns>True if values have changed.</returns>
        /// <remarks>Results are placed into the public field IRValues[]</remarks>
        public bool PollIR()
        {
            bool changed = false;
            byte[] buf = { (byte)'N' };

            // Send the package and wait for the response
            if (!SendPacket(buf, 0))
                return false;

            // Process the result
            int i, j, val;
            j = 0;
            val = 0;

            // If the echoed command does not match there is a problem!
            if (_response[0] != (byte)'n')
            {
                Console.WriteLine("Invalid response to Get IR sensors command");
                return false;
            }

            // Append a comma because it makes processing easier
            if (_response[_responseCount - 1] != (byte)',')
            {
                _response[_responseCount] = (byte)',';
                _responseCount++;
            }
            // Convert each number to binary
            for (i = 2; i < _responseCount && j < IR_COUNT; i++)
            {
                if (_response[i] == (byte)',')
                {
                    IRValues[j] = val;
                    j++;
                    val = 0;
                }
                else
                {
                    val = val * 10 + (int)(_response[i] - (byte)'0');
                }
            }

            // See if any of the IR sensor values have changed
            // NOTE: IR sensors can be a little noisy, so we have a
            // threshold here for changes
            for (i=0; i<IR_COUNT; i++)
            {
                if (Math.Abs(IRValues[i] - priorIRValues[i]) > IR_NOTIFICATION_THRESHOLD)
                {
                    changed = true;
                    break;
                }
            }
            if (changed)
            {
                // Remember the current values
                for (i = 0; i < IR_COUNT; i++)
                    priorIRValues[i] = IRValues[i];
                return true;
            }
            else
                return false;
        }

        /// <summary>
        /// Poll the Switches
        /// </summary>
        /// <returns>True if the switches have changed</returns>
        /// <remarks>Results are placed into the public field Switches</remarks>
        public bool PollSwitches()
        {
            byte[] buf = { (byte)'I' };
            // Send the package and wait for the response
            if (!SendPacket(buf, 0))
                return false;

            // Process the result
            int i, j;
            j = 0;

            if (_response[0] != (byte)'i')
            {
                Console.WriteLine("Invalid response to Get Switches");
                return false;
            }

            // Append a comma because it makes processing easier
            if (_response[_responseCount - 1] != (byte)',')
            {
                _response[_responseCount] = (byte)',';
                _responseCount++;
            }
            // Convert each number to binary
            Switches = 0;
            for (i=2,j=0; i < _responseCount && j < SWITCH_COUNT; i+=2,j++)
            {
                if (_response[i] == (byte)'1')
                    Switches = Switches | (1 << j);
            }

            if (Switches != priorSwitches)
            {
                // Remember the current values
                priorSwitches = Switches;
                return true;
            }
            else
                return false;
        }
        */

        #endregion

        #region Control

        // Turns the speaker on for the specified duration
        private void SpeakerTone(int ms, int Hz)
        {
            SpeakerState(true);

            // Set up a timer for the specified interval
            //            SpawnIterator<int>(ms, SpeakerTimer);

            // Wait for the duration
            Thread.Sleep(ms);

            // Turn off the sound now
            SpeakerState(false);
        }

        /// <summary>
        /// Turn Buzzer On/Off
        /// </summary>
        /// <param name="status"></param>
        public void SpeakerState(bool status)
        {
            // Speaker ON command
            byte[] buf = { (byte)'H', (byte)',', (byte)'1' };

            if (!status)
            {
                // Change ON to OFF
                buf[2] = (byte)'0';
            }
            // Post the message and forget about it
            Command cmd = new Command(buf);
            CommandPort.Post(cmd);
        }

//        private IEnumerable<ITask> SpeakerTimer(int duration)
//        {
//            // Wait the specified period
//            yield return Arbiter.Receive(false, TimeoutPort(duration), delegate(DateTime now) { SpeakerState(false); });
//
//        }

        /// <summary>
        /// Set the LEDs
        /// </summary>
        /// <param name="states"></param>
        public Port<byte[]> SetLEDs(int states)
        {
            int mask = 1;
            byte[] buf = new byte[9];
            buf[0] = (byte)'L';
            for (int i = 0; i < 4; i++)
            {
                buf[i * 2 + 1] = (byte)',';
                if ((states & mask) != 0)
                    buf[i * 2 + 2] = (byte)'1';
                else
                    buf[i * 2 + 2] = (byte)'0';
                mask = mask << 1;
            }

            // Remember the settings because we need them for the
            // Fast Binary Write command
            LEDs = states;

            Command cmd = new Command(buf);
            CommandPort.Post(cmd);
            // Don't care about the response?
            return cmd.ResponsePort;
        }

        /*
        private void SetSingleLED(int num, bool state)
        {
            if (num > 3)
                return;
            int mask = 1;
            for (int i = 0; i < num; i++)
                mask = mask << 1;
            if (state)
                mask = LEDs | mask;
            else
                mask = LEDs & (~mask);
            SetLEDs(mask);
        }
        */

        #endregion

        #region Communication

        /// <summary>
        /// Serial Port Command Handler
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        /// NOTE: This handler executes serial commands which block the thread
        private IEnumerator<ITask> CommandHandler(Command cmd)
        {
            byte[] response = new byte[0];

            if (cmd.CommandString == null || cmd.CommandString.Length == 0)
            {
                // Pathological case -- no command!
                // Somebody sent us an empty packet, so send back an empty response
                cmd.ResponsePort.Post(response);
            }
            else
            {
                // Send the packet and wait for a response
                SendPacket(cmd.CommandString, cmd.ResponseLength);

                // Got a response?
                if (_responseCount > 0)
                {
                    byte[] resp = new byte[_responseCount];
                    for (int i = 0; i < _responseCount; i++)
                        resp[i] = _response[i];
                    cmd.ResponsePort.Post(resp);
                }
                else
                    cmd.ResponsePort.Post(response);
            }

            // Wait one time for a new Command
            Arbiter.Activate(_taskQueue, Arbiter.ReceiveWithIterator<Command>(false, CommandPort, CommandHandler));

            yield break;
        }


        // Carriage Return
        static char[] CR = { '\r' };

        private DateTime _lastPacketTime;

        /// <summary>
        /// Send Packet
        /// </summary>
        /// <param name="buf"></param>
        /// <param name="expectedLength"></param>
        /// <returns></returns>
        // Baud Rate is 115200 which is approximately 11520 chrs/sec
        // which is about a 0.1 of a millisecond per character.
        // NOTE: SendPacket() operates SYNCHRONOUSLY, although it does
        // have timeouts to prevent it from hanging forever.
        private bool SendPacket(byte[] buf, int expectedLength)
        {
            TimeSpan timeDiff;
            int msDiff;
            bool ok = true;

            // Always check! There could be outstanding messages when the
            // connection is closed.
            if (!_serialPort.IsOpen)
                return false;

            // Check that this packet is not too close to the last one.
            // It is ESSENTIAL that there is a delay after sending
            // a command because the Integrator cannot process the
            // commands very quickly and will start to lose data!
            timeDiff = (DateTime.Now - _lastPacketTime);
            msDiff = timeDiff.Milliseconds;
            if (msDiff < Delay)
            {
                // Wait for the difference in time
                Thread.Sleep(Delay - msDiff);
            }

            _responseCount = 0;

            try
            {
                int i;
                // Throw away any left-over data in the input buffer
                _serialPort.DiscardInBuffer();

                // Output the packet one byte at a time to give the
                // Integrator time to digest the characters.
                // Otherwise it gets a data overrun and locks up!
                for (i = 0; i < buf.Length; i++)
                {
                    _serialPort.Write(buf, i, 1);
                    Thread.Sleep(10);
                }
                // Append a Carriage Return (CR) which is what triggers the
                // command interpreter on the Hemisson
                _serialPort.Write(CR, 0, 1);
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Write Error on command " +
                    (char)buf[0] + " (Error count " + ErrorCounter + ")");
                Console.WriteLine("Exception: " + ex.Message);
                // Try to re-synch
                _serialPort.Write(CR, 0, 1);
                ok = false;
            }
            finally
            {
                _lastPacketTime = DateTime.Now;
            }
            if (!ok)
                return false;

            ok = true;
            try
            {
                int i, val;

                i = 0;
                if (expectedLength != 0)
                {
                    // The response is of a defined length so read exactly
                    // that many characters or time out.
                    // This is necessary for binary data which might just
                    // happen to contain a CR character.
                    for (i=0; i<expectedLength; i++)
                    {
                        val = _serialPort.ReadByte();
                        _response[i] = (byte)val;
                    }
                    // Ignore the final CR ...
                }
                else
                {
                    while ((val = _serialPort.ReadByte()) != '\r')
                    {
                        _response[i] = (byte)val;
                        i++;
                    }
                }
                _responseCount = i;

                // Throw away the rest of the response (only a LF)
                _serialPort.DiscardInBuffer();
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Read Error on command " +
                    (char)buf[0] + " (Error count " + ErrorCounter + ")");
                Console.WriteLine("Exception: " + ex.Message);
                // Send a CR to try to re-synch
                _serialPort.Write(CR, 0, 1);
                ok = false;
            }
            finally
            {
                _lastPacketTime = DateTime.Now;
            }
            if (!ok)
                return false;

            MessagesSentCounter++;
            return true;
        }

        /*
        // Carriage Return
        static char[] CR = { '\r' };

        // SendPacket()
        // Baud Rate is 115200 which is approximately 11520 chrs/sec
        // which is only a fraction of a millisecond per character
        // NOTE: SendPacket() operates SYNCHRONOUSLY, although it does
        // have timeouts to prevent it from hanging forever.
        private bool SendPacket(byte[] buf)
        {
            try
            {
                int i;
                // Throw away any left-over data in the input buffer
                _serialPort.DiscardInBuffer();

                // Output the packet one byte at a time to give the
                // Hemisson time to digest the characters.
                // Otherwise it gets a data overrun and locks up!
                for (i = 0; i < buf.Length; i++)
                {
                    _serialPort.Write(buf, i, 1);
                    Thread.Sleep(1);
                }

                // Output the packet
                //_serialPort.Write(buf, 0, buf.Length);
                //
                // Hang here until the output buffer has emptied
                // This should be virtually instantaneous
                //while (_serialPort.BytesToWrite > 0)
                //    Thread.Sleep(1);

                // Append a Carriage Return (CR) which is what triggers the
                // command interpreter on the Hemisson
                _serialPort.Write(CR, 0, 1);
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Write Error: " + ex.ToString());
                _serialPort.Write(CR, 0, 1);
                return false;
            }

            // It is ESSENTIAL that there is a delay after sending
            // a command because the Hemisson cannot process the
            // commands very quickly and will start to lose data!
            // The robot will HANG if you send another command before
            // it has finished processing the previous one. This
            // appears to be a bug in the Hemisson code probably due
            // to a data overrun not being cleared.
            Thread.Sleep(50);

            try
            {
                int i, val;

                i = 0;
                while ((val = _serialPort.ReadByte()) != '\r')
                {
                    _response[i] = (byte)val;
                    i++;
                }
                _responseCount = i;

                // Throw away the rest of the response (only a LF)
                _serialPort.DiscardInBuffer();
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Read Error: " + ex.ToString());
                // Send a CR to try to re-synch
                _serialPort.Write(CR, 0, 1);
                return false;
            }

            MessagesSentCounter++;
            return true;
        }
         */

        #endregion
    }

    /// <summary>
    /// Serial Command
    /// </summary>
    /// <remarks>Used internally to send packets to the serial port</remarks>
    public class Command
    {
        public byte[] CommandString;        // Command to send
        public int ResponseLength;          // Expected length of response (before CR LF)
                                            // 0 = All data up to CR (no good for binary)
        public Port<byte[]> ResponsePort;   // Port to send response back on

        public Command()
        {
            CommandString = null;
            ResponseLength = 0;
            // Make sure that there is always a response port
            ResponsePort = new Port<byte[]>();
        }

        public Command(byte[] buf)
        {
            CommandString = buf;
            ResponseLength = 0;
            ResponsePort = new Port<byte[]>();
        }
    }

}
